#include "Align.h"
#include "Agent.h"
#include "MathUtility.h"
#include "SteeringOutput.h"

namespace Engine
{
	Align::Align():m_slowRadius(0.0f), m_targetRadius(0.0f), m_timeToTarget(0.0f){}
	void Align::Update(SteeringOutput* output)
	{
		float rotation = MapToPiRange(m_target ->GetRot() - m_parent ->GetRot());
		float rotationSize = std::fabsf(rotation);

		if(rotationSize < m_targetRadius) return;

		float targetRot = 0.0f;

		if(rotationSize > m_slowRadius) 
			targetRot = m_parent ->GetMaxAngularVel();

		else
			targetRot = m_parent ->GetMaxAngularVel() * rotationSize / m_slowRadius;

		targetRot *= rotation / rotationSize;

		output ->angular = targetRot - m_parent ->GetRot();
		output ->angular /= m_timeToTarget;

		float angularAcc = std::fabsf(output ->angular);
		if(angularAcc > m_parent ->GetMaxAngularAcc())
		{
			output ->angular /= angularAcc;
			output ->angular *= m_parent ->GetMaxAngularAcc();
		}
	}
}